/*
The surface-physics project: http://code.google.com/p/surface-physics/
Adds real world physics based on the Bullet physics engine, to bases in the Orbiter space
flight simulation software.

Written by Abhijit Nandy
License : Apache License 2.0

Bump_TerrainPatchOru.cpp

This file implements the terrain patch class.

*/
#include "Bump_TerrainPatchOru.h"


//Orulex types
typedef union{
 double data[4];
 struct {double x,y,z,m;};
}VECTOR4;

extern double ORUGetElevByVec(VECTOR4 pos);
extern VECTOR4 v32v4(VECTOR3 v);



/**
 * Constructor for class, doesn't create terrain
 */
Bump_TerrainPatchOru::Bump_TerrainPatchOru(unsigned int lngMaxPoints, unsigned int latMaxPoints,
									 double lngAngleIncr, double latAngleIncr,
									 double lngBorderWidth, double latBorderWidth,
									 double updateDistance,
									 Bump_BulletBase *base) :
									 Bump_TerrainPatch(lngMaxPoints, latMaxPoints,
									 						lngAngleIncr, latAngleIncr,
									 						lngBorderWidth, latBorderWidth,
									 						updateDistance,
									 						base)
{

	//No child specific initialization yet
}


/**
 * Creates triangle mesh from Orulex data
 */
inline int
Bump_TerrainPatchOru::setTerrainVertices()
{
	unsigned int i, j, k;
	int markerStep = 0;
	double lng, lat, elev;

	//Global direction and position vectors
	VECTOR3 gdirSurfPointRadiusVector;
	VECTOR3 gposMoon, gposSurfPoint, gposElevatedPoint;

	//Relative to vessel(base)
	VECTOR3 rposElevatedPoint;

	aabbMin.setValue( BT_LARGE_FLOAT, BT_LARGE_FLOAT, BT_LARGE_FLOAT);
	aabbMax.setValue(-BT_LARGE_FLOAT,-BT_LARGE_FLOAT,-BT_LARGE_FLOAT);


	oapiWriteLogV("Bump_TerrainPatchOru::setTerrainVertices: CALLED lng=%f, lat=%f,\
	  	  	  lngAngleIncr=%f, latAngleIncr=%f", equLngStart, equLatStart, lngAngleIncr, latAngleIncr);


	//First generate the points matrix in base frame


	//Get reference body(nearest celestial body) co-ordinates, this keeps changing
	oapiGetGlobalPos(bumpBase->hObjRefBody, &gposMoon);

	//lng = -0.582085;
	lng = equLngStart;
	k = -1;
	for(i=0; i<lngMaxPoints; ++i){

		//lat = 0.717760;
		lat = equLatStart;
		for(j=0; j<latMaxPoints; ++j){

			//Must use base radius here as we are getting the gpos for a surface point(for Orulex)
			oapiEquToGlobal(bumpBase->hObjRefBody, lng, lat, bumpBase->radRefBody, &gposSurfPoint);

			//Vector from Moon center to surface point in global frame
			gdirSurfPointRadiusVector = gposSurfPoint - gposMoon;

			//Elevation measured from the surface
			elev = ORUGetElevByVec(v32v4(gdirSurfPointRadiusVector));

			gposElevatedPoint = unit(gdirSurfPointRadiusVector) * elev;  //elevation calculations in global frame, removes 1 function call & 1 mul.

			gposElevatedPoint += gposMoon; //translate above the moon from global origin


			bumpBase->bb->Global2Local(gposElevatedPoint, rposElevatedPoint);

		/*	//No need to add gposBB here, as we are converting the surface point position to base frame
			bumpBase->bb->Global2Local(gposSurfPoint, rposSurfPoint);

			//Get the unit vector in direction of radius vector of the surface point, in base frame
			rdirUnitSurfPointRadiusVector = unit(rdirSurfPointRadiusVector);

			//Add the scaled unit vector to the surface point position in base frame
			//to get elevated point in base frame
			rposElevatedPoint = rposSurfPoint + rdirUnitSurfPointRadiusVector * elev;*/

			//Mesh addition logic
		/*	if( (i%20==0 && j==0) || (i%(MAX_POINTS-1)==0 && j==0) ||
				(i%20==0 && j==(MAX_POINTS-1)) || (i%(MAX_POINTS-1)==0 && j==(MAX_POINTS-1))	)
				bumpBase->bb->AddMesh("Bump/DebugMeshes/ArrowMarker", &rposElevatedPoint);*/


			//----------------- Now Bullet stuff ----------------------



			vertices[i*lngMaxPoints + j].setValue(btScalar(rposElevatedPoint.x),
												btScalar(rposElevatedPoint.y),
												btScalar(rposElevatedPoint.z));

			//Translate the surface point by the sim box offset vector;
			vertices[i*lngMaxPoints + j] += bumpBase->simBoxOffset;

			aabbMax.setMax(vertices[i*lngMaxPoints + j]);
			aabbMin.setMin(vertices[i*lngMaxPoints + j]);

		/*	oapiWriteLogV("Bump: lng = %f(%f), lat = %f(%f) : elev : %f, ANGLE_INCREMENT : %f, rposElevatedPoint = %f,%f,%f, \
					aabbMax=(%f,%f,%f), aabbMin=(%f,%f,%f)",
					lng, (lng*180/PI), lat, (lat*180/PI), elev, ANGLE_INCREMENT, V3ARGS(rposElevatedPoint),
					aabbMax.x(), aabbMax.y(), aabbMax.z(), aabbMin.x(), aabbMin.y(), aabbMin.z());
			*/

			//Generate indices for 1 quad at a time, 2 triangles per quad.
			//i & j should go upto MAX_POINTS - 2 = MAX_QUADS - 1 => i,j < MAX_QUADS
			if(i<lngMaxQuads && j<latMaxQuads){
				//1st triangle of this quad
				indices[++k] = i*lngMaxPoints + j;			//0
				indices[++k] = i*lngMaxPoints + (j+1);		//1
				indices[++k] = (i+1)*lngMaxPoints + j;		//3

				indices[++k] = (i+1)*lngMaxPoints + j;		//3
				indices[++k] = i*lngMaxPoints + (j+1);		//1
				indices[++k] = (i+1)*lngMaxPoints + (j+1);	//4
			}


			lat += latAngleIncr;

		}//for-j

		lng += lngAngleIncr;
	}//for-i



	//Points recorded, print to chk
/*	for(i=0; i<MAX_POINTS; i++){
		for(j=0; j<MAX_POINTS; j++){

			//aabbMax.setMax(vertices[i*MAX_POINTS + j]);
			//aabbMin.setMin(vertices[i*MAX_POINTS + j]);
			//aabbMax.setValue(10000,10000,10000);
			//aabbMin.setValue(-10000,-10000,-10000);

			oapiWriteLogV("Bump: vertices[%d] = %f, %f, %f, aabbMin(%f,%f,%f), aabbMax(%f,%f,%f)",
				i*MAX_POINTS +j,
				vertices[i*MAX_POINTS +j].x(),
				vertices[i*MAX_POINTS +j].y(),
				vertices[i*MAX_POINTS +j].z(),
				aabbMin.x(), aabbMin.y(), aabbMin.z(),
				aabbMax.x(), aabbMax.y(), aabbMax.z());

		}
	}
*/

	//Print indices
/*	for( k=0; k<(MAX_QUADS*MAX_QUADS*3*2); k+=(3*2) ){

		oapiWriteLogV("Bump: -----Quad : %d-----", k/6);

		oapiWriteLogV("Bump: Triangle : %d : indices[%d] = %d, indices[%d] = %d, indices[%d] = %d",
			k/3,
			k,   indices[k],
			k+1, indices[k+1],
			k+2, indices[k+2] );

		oapiWriteLogV("Bump: Triangle : %d : indices[%d] = %d, indices[%d] = %d, indices[%d] = %d",
			(k+3)/3,
			(k+3),   indices[(k+3)],
			(k+3)+1, indices[(k+3)+1],
			(k+3)+2, indices[(k+3)+2] );

	}
*/
	//Print covered lola range
	lng -= lngAngleIncr;
	lat -= latAngleIncr;

	//oapiWriteLogV("Bump: COVERED : lola(%f,%f) TO lola(%f,%f)", equLngStart, equLatStart, lng, lat);


	//Check if lola ended as expected
	//TODO: Comment out this check later
	if(lat != equLatEnd || lng != equLngEnd){
		oapiWriteLogV("Bump: COVERED : ERROR Expected lola to cover till lo:%f, la:%f", equLngEnd, equLatEnd);
	}




	return 0;
}

